inline double x_of(const Eta& e) {
    return e.x;
}
inline double y_of(const Eta& e) {
    return e.y;
}
inline double z_of(const Eta& e) {
    return e.z;
}

template <PositionLike T>
inline Eigen::Vector3d pos_vector(const T& t) {
    return Eigen::Vector3d{x_of(t), y_of(t), z_of(t)};
}

template <PoseLike T>
geometry_msgs::msg::Pose pose_like_to_pose_msg(const T& pose_like) {
    geometry_msgs::msg::Pose pose;

    pose.position.x = x_of(pose_like);
    pose.position.y = y_of(pose_like);
    pose.position.z = z_of(pose_like);

    if constexpr (QuatPoseLike<T>) {
        pose.orientation.w = qw_of(pose_like);
        pose.orientation.x = qx_of(pose_like);
        pose.orientation.y = qy_of(pose_like);
        pose.orientation.z = qz_of(pose_like);
    } else if constexpr (EulerPoseLike<T>) {
        const auto q = vortex::utils::math::euler_to_quat(
            roll_of(pose_like), pitch_of(pose_like), yaw_of(pose_like));
        pose.orientation.w = q.w();
        pose.orientation.x = q.x();
        pose.orientation.y = q.y();
        pose.orientation.z = q.z();
    }

    return pose;
}